#include "motor.h"
#include "C_H.h"

// ���õ�������ķ����PWM
void set_motor_control(const char *motor_dir_pin, const char *motor_pwm_pin, int pid_output)
{
    if (pid_output >= 0)
    {
        gpio_set_level(motor_dir_pin, 0); // ��ת
        pwm_set_duty(motor_pwm_pin, pid_output);
    }
    else
    {
        gpio_set_level(motor_dir_pin, 1); // ��ת
        pwm_set_duty(motor_pwm_pin, -pid_output);
    }
}


#ifdef yidui

// �������ҵ��
void control_motors(int pid_output_l, int pid_output_r)
{
    // ��������
    // set_motor_control(MOTOR1_DIR, MOTOR1_PWM, pid_output_l * (-1));
    set_motor_control(MOTOR1_DIR, MOTOR1_PWM, pid_output_r* (-1));

    // �ҵ������
    set_motor_control(MOTOR2_DIR, MOTOR2_PWM, pid_output_l* (-1));
}

#endif

#ifdef erdui

// �������ҵ��
void control_motors(int pid_output_l, int pid_output_r)
{
    // ��������
    // set_motor_control(MOTOR1_DIR, MOTOR1_PWM, pid_output_l * (-1));
    set_motor_control(MOTOR1_DIR, MOTOR1_PWM, pid_output_r* (-1));

    // �ҵ������
    set_motor_control(MOTOR2_DIR, MOTOR2_PWM, pid_output_l* (-1));
}

#endif